/**
  ******************************************************************************
  * @file    main.cc
  * @author  xinkai yu
  * @version V1.0.1
  * @date    2022/02/22
  * @brief   arm interface for MobiRo gen3 @ TIB330
  ******************************************************************************
  * @attention
  *
  ******************************************************************************
  */

#include <chrono>
#include "ros/ros.h"
#include "sensor_msgs/JointState.h"
#include "geometry_msgs/TransformStamped.h"
#include "std_srvs/SetBool.h"
#include "std_srvs/Empty.h"
#include "tf/tf.h"
#include "tf/transform_listener.h"
#include "arm_base/joint_angular.h"
#include "udp_client.h"
#include "std_msgs/Float64MultiArray.h" 

class ArmUdpHandler: public UdpHandler {
 public://这个就是整个arm的收发程序
  explicit ArmUdpHandler(size_t buffer_length): UdpHandler(buffer_length) {
    //这个是两个发布，一个是标准发布，urdf要用的，一个是机械臂的末端姿态
    arm_conf_pub_ = nh_.advertise<sensor_msgs::JointState>(
        "joint_states", 5, this);//反馈joint的状态，这个直接会映射到urdf里
    hand_pose_pub_ = nh_.advertise<geometry_msgs::TransformStamped>(
        "hand_pose", 5, this);//反馈手末端姿态

    //要到达目标点的关节逆解，直接发给udp，这个在发之前会自己判断该不该发出去
    target_conf_sub_ = nh_.subscribe<arm_base::joint_angular>(
        "target_conf", 1, &ArmUdpHandler::TargetConfCallback, this);

    nav_conf_sub_ = nh_.subscribe<arm_base::joint_angular>(
        "nav_target_conf", 1, &ArmUdpHandler::NavTargetConfCallback, this);

    Nav_conf_sub_ = nh_.subscribe<std_msgs::Float64MultiArray>(
        "nav_conf", 1, &ArmUdpHandler::NavConfCallback, this);

    //这是创建了服务，让爪子开启或者闭合
    set_gripper_status_ = nh_.advertiseService(
        "arm_base/set_gripper_status", &ArmUdpHandler::GripCallback, this);

    //call了三个服务，得找到服务的回调
    //服务的客户端，这是yolo的开关
    detector_switch_sc_ = nh_.serviceClient<std_srvs::SetBool>(
        "arm_detector/set_detect_status");
    // gpd_switch_sc_=nh_.serviceClient<std_srvs::SetBool>(
    //     "detect_grasps/set_gpd_status");

   
    enable_grip_sc_ = nh_.serviceClient<std_srvs::Empty>(
        "arm_kinematic/grip_enable");
    enable_nav_sc_ = nh_.serviceClient<std_srvs::Empty>(
        "base_nav/nav_enable");

    nav_complete_sc_ = nh_.serviceClient<std_srvs::Empty>(
        "base_nav/unlock");


    go_default_sc_ = nh_.serviceClient<std_srvs::Empty>(
        "arm_kinematic/go_default");

    //这个是upd接受消息处理，同时调用detect的服务
    conf_msg_handle_thread_ = std::thread(&ArmUdpHandler::ArmConfHandle, this);

    //发布状态的线程，不放在上一个线程可能是因为，频率不同吧
    joint_state_update_thread_ = std::thread(
        &ArmUdpHandler::JointStateUpdate, this);

    //发布手的状态矩阵
    hand_moniter_thread_ = std::thread(&ArmUdpHandler::HandPoseMoniter, this);

    overtime_threshold_ = 20.0;
    action_ongoing_flag_ = false;
    arm_conf_.name.push_back("joint1");
    arm_conf_.name.push_back("joint2");
    arm_conf_.name.push_back("joint3");
    arm_conf_.name.push_back("joint6");
    for (int i = 0; i < 4; i++) {
      arm_conf_.position.push_back(0.0);
    }
  }
  
  ~ArmUdpHandler() {
    if (conf_msg_handle_thread_.joinable())
      conf_msg_handle_thread_.join();
    if (joint_state_update_thread_.joinable()) 
      joint_state_update_thread_.join();
    if (hand_moniter_thread_.joinable())
      hand_moniter_thread_.join();
  }

  void JointStateUpdate() {//这是发送机械臂状态的线程，不过为什么要分开呢，这个时间戳是发布时间戳，而不是接收到的，所以有延迟
    ros::Rate loop_rate(50);
    while(ros::ok()) {
      joint_mutex_.lock();
      arm_conf_.header.stamp = ros::Time::now();
      arm_conf_pub_.publish(arm_conf_);
      joint_mutex_.unlock();
      loop_rate.sleep();
    }
  }

  void ArmConfHandle() {
    ros::Rate loop_rate(100);
    ros::Time action_stamp = ros::Time::now();
    while(ros::ok()) {
      if ((ros::Time::now() - action_stamp).toSec() > overtime_threshold_ &&
          action_ongoing_flag_) {
        action_ongoing_flag_ = false;
        std_srvs::SetBool detect_switch_cmd;
        detect_switch_cmd.request.data = true;  // enable detect允许抓取
        detector_switch_sc_.call(detect_switch_cmd);//设置检测状态
        // std_srvs::SetBool gpd_switch_cmd;
        // gpd_switch_cmd.request.data=true;
        // gpd_switch_sc_.call(gpd_switch_cmd);
        std::cout << "Action overtime!!! " 
                  << detect_switch_cmd.response.message 
                  << std::endl;//显示超时的信息
      }  // overtime handler
      
      std::unique_lock<std::mutex> lock(recv_mutex_);
      if (!recv_frame_.size()) continue;
      double joint_angle = 0.0;
      switch (recv_frame_.at(0)) {//这是处理收到的udp消息，然后发出去
        case 0x02://运行过程中，不允许检测
          // std::cout << (int)recv_frame_.at(1) << std::endl;
          if (recv_frame_.at(1) == 0x01) {
            action_ongoing_flag_ = true;
            std_srvs::SetBool detect_switch_cmd;//把检测弄成服务了，这是不允许检测
            detect_switch_cmd.request.data = false;  // disable detect不允许检测
            detector_switch_sc_.call(detect_switch_cmd);
            // std_srvs::SetBool gpd_switch_cmd;
            // gpd_switch_cmd.request.data=false;
            // gpd_switch_sc_.call(gpd_switch_cmd);
            std::cout << "Arm action begin! " 
                      << detect_switch_cmd.response.message 
                      // << gpd_switch_cmd.response.message
                      << std::endl;
            action_stamp = ros::Time::now();
          }
          else if (recv_frame_.at(1) == 0x02) {
            action_ongoing_flag_ = false;//不是运行就允许检测
            std_srvs::SetBool detect_switch_cmd;
            detect_switch_cmd.request.data = true;  // enable detect运行检测
            detector_switch_sc_.call(detect_switch_cmd);
            // std_srvs::SetBool gpd_switch_cmd;
            // gpd_switch_cmd.request.data=true;
            // gpd_switch_sc_.call(gpd_switch_cmd);
            std::cout << "Arm action done! "
                      << detect_switch_cmd.response.message 
                      // << gpd_switch_cmd.response.message
                      << std::endl;
          }
          else {
            action_ongoing_flag_ = false;
            std::cout << "Wait for init!" << std::endl;
          }
          break;
        case 0x0a://反馈机械臂状态
          joint_mutex_.lock();
          arm_conf_.position.clear();
          joint_angle = 
            -(int16_t)(recv_frame_.at(2) << 8 | recv_frame_.at(1)) / 100.0;
          arm_conf_.position.push_back(joint_angle * M_PI / 180.0);
          joint_angle = 
            -(int16_t)(recv_frame_.at(4) << 8 | recv_frame_.at(3)) / 100.0;
          arm_conf_.position.push_back(joint_angle * M_PI / 180.0);
          joint_angle = 
            -(int16_t)(recv_frame_.at(6) << 8 | recv_frame_.at(5)) / 100.0;
          arm_conf_.position.push_back(joint_angle * M_PI / 180.0);
          arm_conf_.position.push_back(0.0);
          joint_mutex_.unlock();
          break;
        case 0xa1:
          // std::cout << (int)recv_frame_.at(1) << std::endl;
          if (recv_frame_.at(1) == 0x00) {
            // std_srvs::Empty go_default_cmd;  // go default
            // go_default_sc_.call(go_default_cmd);
            ROS_INFO("send invers to arm");
            std_srvs::Empty complete_cmd;
            nav_complete_sc_.call(complete_cmd);
            std::unique_lock<std::mutex> lock(send_mutex_);
            send_buf_[0] = 0x55;
            send_buf_[1] = 0xaa;
            send_buf_[2] = 0x09;
            send_buf_[3] = 0x01;
            send_buf_[4] = (int16_t)(target_msg.waist * 100) & 0xff;
            send_buf_[5] = (int16_t)(target_msg.waist * 100) >> 8;
            send_buf_[6] = (int16_t)(target_msg.shoulder * 100) & 0xff;
            send_buf_[7] = (int16_t)(target_msg.shoulder * 100) >> 8;
            send_buf_[8] = (int16_t)(target_msg.elbow * 100) & 0xff;
            send_buf_[9] = (int16_t)(target_msg.elbow * 100) >> 8;
            send_buf_[10] = (int16_t)(target_msg.wrist * 100) & 0xff;  // wrist joint
            send_buf_[11] = (int16_t)(target_msg.wrist * 100) >> 8;
            send_buf_[12] = 0x00;  // sum check
            for (int i = 3; i < 12; i++) {
              send_buf_[12] += send_buf_[i];
            }
            frame_length_ = 13;
            send_flag_ = true;
            overtime_threshold_ = target_msg.life_time;
            std_srvs::Empty grip_enable_cmd;
            enable_grip_sc_.call(grip_enable_cmd);  // enable auto grasp 这俩用来callenable
            std_srvs::Empty nav_enable_cmd;
            enable_nav_sc_.call(nav_enable_cmd); 

          }
          else if (recv_frame_.at(1) == 0x01) {
            // std_srvs::Empty grip_enable_cmd;
            // enable_grip_sc_.call(grip_enable_cmd);  // enable auto grasp 这俩用来callenable
            // std_srvs::Empty nav_enable_cmd;
            // enable_nav_sc_.call(nav_enable_cmd); 
            std::unique_lock<std::mutex> lock(send_mutex_);
            send_buf_[0] = 0x55;
            send_buf_[1] = 0xaa;
            send_buf_[2] = 0x02;
            send_buf_[3] = 0x08;
            send_buf_[4] = 0x02;
            send_buf_[5] = 0x0a;
            frame_length_ = 6;
            send_flag_ = true;
            ROS_INFO("Gripper close msg sent!");
          }
          else {}
          break;
        case 0xb1:
          if(recv_frame_.at(1) == 0x01){
            ROS_INFO("send invers to arm");
            std_srvs::Empty complete_cmd;
            nav_complete_sc_.call(complete_cmd);
            std::unique_lock<std::mutex> lock(send_mutex_);
            send_buf_[0] = 0x55;
            send_buf_[1] = 0xaa;
            send_buf_[2] = 0x09;
            send_buf_[3] = 0x01;
            send_buf_[4] = (int16_t)(target_msg.waist * 100) & 0xff;
            send_buf_[5] = (int16_t)(target_msg.waist * 100) >> 8;
            send_buf_[6] = (int16_t)(target_msg.shoulder * 100) & 0xff;
            send_buf_[7] = (int16_t)(target_msg.shoulder * 100) >> 8;
            send_buf_[8] = (int16_t)(target_msg.elbow * 100) & 0xff;
            send_buf_[9] = (int16_t)(target_msg.elbow * 100) >> 8;
            send_buf_[10] = (int16_t)(target_msg.wrist * 100) & 0xff;  // wrist joint
            send_buf_[11] = (int16_t)(target_msg.wrist * 100) >> 8;
            send_buf_[12] = 0x00;  // sum check
            for (int i = 3; i < 12; i++) {
              send_buf_[12] += send_buf_[i];
            }
            frame_length_ = 13;
            send_flag_ = true;
            overtime_threshold_ = target_msg.life_time;
            std_srvs::Empty grip_enable_cmd;
            enable_grip_sc_.call(grip_enable_cmd);  // enable auto grasp 这俩用来callenable
            std_srvs::Empty nav_enable_cmd;
            enable_nav_sc_.call(nav_enable_cmd); 
            std_srvs::SetBool detect_switch_cmd;
            detect_switch_cmd.request.data = true;  // enable detect运行检测
            detector_switch_sc_.call(detect_switch_cmd);
          }
          break;
        default: break;
      }

      recv_frame_.clear();
      lock.unlock();      
      loop_rate.sleep();
    }
    exit_flag_ = true;//这是当这个线程结束的时候，UDP也就结束了
  }

  void HandPoseMoniter() {
    ros::Rate loop_rate(100);
    while (ros::ok()) {
      tf::StampedTransform transform;
      try {
        tf_listener_.lookupTransform("/base_link", "/grasping_frame", 
                                     ros::Time(0), transform);
      }
      catch (tf::TransformException &ex) {
        ROS_ERROR("%s",ex.what());
        ros::Duration(1.0).sleep();
        continue;
      }
      geometry_msgs::TransformStamped hand_pose;
      tf::transformStampedTFToMsg(transform, hand_pose);
      hand_pose_pub_.publish(hand_pose);
      loop_rate.sleep();
    }
  }

 private:
  void TargetConfCallback(const arm_base::joint_angular::ConstPtr &conf) {
    if (action_ongoing_flag_) {
      ROS_WARN("Action ongoing!!!");
      return;
    }
    //在这等待20秒呢
    std::unique_lock<std::mutex> lock(send_mutex_);
    send_buf_[0] = 0x55;
    send_buf_[1] = 0xaa;
    send_buf_[2] = 0x09;
    send_buf_[3] = 0x01;
    send_buf_[4] = (int16_t)(conf->waist * 100) & 0xff;
    send_buf_[5] = (int16_t)(conf->waist * 100) >> 8;
    send_buf_[6] = (int16_t)(conf->shoulder * 100) & 0xff;
    send_buf_[7] = (int16_t)(conf->shoulder * 100) >> 8;
    send_buf_[8] = (int16_t)(conf->elbow * 100) & 0xff;
    send_buf_[9] = (int16_t)(conf->elbow * 100) >> 8;
    send_buf_[10] = 0x00;  // wrist joint
    send_buf_[11] = 0x00;
    send_buf_[12] = 0x00;  // sum check
    for (int i = 3; i < 12; i++) {
      send_buf_[12] += send_buf_[i];
    }
    frame_length_ = 13;
    send_flag_ = true;
    overtime_threshold_ = conf->life_time;
  }

    void NavTargetConfCallback(const arm_base::joint_angular::ConstPtr &conf) {
      ROS_INFO("Recive arm angle!");
      target_msg = *conf;
    }


  void NavConfCallback(const std_msgs::Float64MultiArray::ConstPtr &conf){
    if (action_ongoing_flag_) {
      ROS_WARN("Nav or arm ongoing!!!");
      return;
    }
    std::unique_lock<std::mutex> lock(send_mutex_);
    send_buf_[0] = 0x55;
    send_buf_[1] = 0xaa;
    send_buf_[2] = 0x07;//长度
    send_buf_[3] = 0xc1;

    send_buf_[4] = (int16_t)(conf->data[0] * 100) & 0xff;
    send_buf_[5] = (int16_t)(conf->data[0] * 100) >> 8;// need to know how to trans nav points.
    send_buf_[6] = (int16_t)(conf->data[1] * 100) & 0xff;
    send_buf_[7] = (int16_t)(conf->data[1] * 100) >> 8;
    send_buf_[8] = (int16_t)(conf->data[2] * 100) & 0xff;
    send_buf_[9] = (int16_t)(conf->data[2] * 100) >> 8;
    send_buf_[10] = 0x00;
    for (int i = 3; i < 10; i++) {
      send_buf_[10] += send_buf_[i];
    }
    frame_length_ = 11;
    send_flag_ = true;
    std_srvs::SetBool detect_switch_cmd;
    detect_switch_cmd.request.data = false;  // enable detect运行检测
    detector_switch_sc_.call(detect_switch_cmd);
    ROS_INFO("Nav Udp send");
  }

  bool GripCallback(std_srvs::SetBoolRequest &request,
                    std_srvs::SetBoolResponse &response) {//当服务发出来消息后，判断是真假，进而决定开还是合
    if (request.data == true) {
      std::unique_lock<std::mutex> lock(send_mutex_);
      send_buf_[0] = 0x55;
      send_buf_[1] = 0xaa;
      send_buf_[2] = 0x02;
      send_buf_[3] = 0x08;
      send_buf_[4] = 0x02;
      send_buf_[5] = 0x0a;
      frame_length_ = 6;
      send_flag_ = true;
      response.success = true;
      response.message = "Gripper close msg sent!";
    }
    else if (request.data == false) {
      std::unique_lock<std::mutex> lock(send_mutex_);
      send_buf_[0] = 0x55;
      send_buf_[1] = 0xaa;
      send_buf_[2] = 0x02;
      send_buf_[3] = 0x08;
      send_buf_[4] = 0x01;
      send_buf_[5] = 0x09;
      frame_length_ = 6;
      send_flag_ = true;
      response.success = true;
      response.message = "Gripper opene msg sent!";
    }
    return true;
  }

  ros::NodeHandle nh_;
  ros::Publisher arm_conf_pub_;
  ros::Publisher hand_pose_pub_;
  ros::Subscriber target_conf_sub_;
  ros::Subscriber nav_conf_sub_;
  ros::Subscriber Nav_conf_sub_;
  ros::ServiceServer set_gripper_status_;
  ros::ServiceClient detector_switch_sc_;
  ros::ServiceClient enable_grip_sc_;
  ros::ServiceClient enable_nav_sc_;
  ros::ServiceClient nav_complete_sc_;
  ros::ServiceClient go_default_sc_;
  // ros::ServiceClient gpd_switch_sc_;
  std::thread conf_msg_handle_thread_;
  std::thread joint_state_update_thread_;
  std::thread hand_moniter_thread_;
  sensor_msgs::JointState arm_conf_;
  std::mutex joint_mutex_;
  tf::TransformListener tf_listener_;
  double overtime_threshold_;
  bool action_ongoing_flag_;
  arm_base::joint_angular target_msg;
};

string UdpHandler::dst_ip_addr = "192.168.1.10";  // target
string UdpHandler::dst_ip_addr2 = "192.168.1.102";
string UdpHandler::src_ip_addr = "192.168.1.135";  // local
unsigned short UdpHandler::dst_port = 7777;
unsigned short UdpHandler::dst_port2 = 9696;
unsigned short UdpHandler::src_port = 8080;

int main(int argc, char **argv) {
  ros::init(argc, argv, "arm_base");
  std::unique_ptr<ArmUdpHandler> udp_handler = 
    std::make_unique<ArmUdpHandler>(256);
  ros::spin();
  return 0;
}
